/*
*Copyright (c) Nikhil Jain.
*Project ICARUS, communications and 
*artificial intelligence.
*/



/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
 */
package javadrone.client.simulation;

//~--- non-JDK imports --------------------------------------------------------

import com.lti.civil.CaptureException;

import javadrone.Drone;
import javadrone.RemoteImage;
import javadrone.Server;

import javadrone.client.CommandConsumer;
import javadrone.client.access.CamImageProvider;
import javadrone.client.access.ImageProducer;
import javadrone.client.access.ImageProvider;
import javadrone.client.access.ImageQueue;

import javadrone.command.CommandExecutor;
import javadrone.command.ExecutionStatus;

import javadrone.remotepackage.RemoteBootPackage;

import javadrone.statedata.BootData;
import javadrone.statedata.BootData.BootState.BootFlag;
import javadrone.statedata.ErrorData;
import javadrone.statedata.ErrorData.ErrorState.ErrorFlag;

import simbad.demo.Demo;

//~--- JDK imports ------------------------------------------------------------

import java.util.logging.Level;
import java.util.logging.Logger;

import javax.vecmath.Vector3d;

/**
 *
 * @author Nikzzay
 */
public class BotSimulator {
    protected BootData.BootState.BootFlag  bootFlag;
    protected CommandExecutor              ce;
    private boolean                        collisionFlag;
    protected Drone                        commandDrone;
    private CommandConsumer                consumer;
    private ErrorData.ErrorState.ErrorFlag errorFlag;
    protected ImageProvider                iP;
    protected ImageQueue                   imageQueue;
    private boolean                        isAlive;
    private boolean                        isHovering;
    private boolean                        isLanding;
    private double                         roll, yaw, pitch;
    private double                         x, y, z;

    public BotSimulator(Drone commandDrone, CommandExecutor ce, int queueSize) throws CaptureException {
        imageQueue        = new ImageProducer(queueSize);
        this.commandDrone = commandDrone;
        this.ce           = ce;
    }

    public BotSimulator(Drone commandDrone, CommandExecutor ce, ImageProvider iP, int queueSize)
            throws CaptureException {
        imageQueue        = new ImageProducer(queueSize);
        this.commandDrone = commandDrone;
        this.ce           = ce;
        this.iP           = iP;
        iP.attach(imageQueue);
    }

    public void attachImageProvider(ImageProvider iP) {
        this.iP = iP;
        iP.attach(imageQueue);
    }

    public ExecutionStatus translateBy(Vector3d translate) {
        System.out.println("Moving...");
        x += translate.x;
        y += translate.y;
        z += translate.z;

        return ExecutionStatus.SUCCESS;
    }

    public ExecutionStatus rotateBy(Vector3d rotate) {
        pitch = (pitch += rotate.x) % 2 * Math.PI;
        roll  = (roll += rotate.y) % 2 * Math.PI;
        yaw   = (yaw += rotate.z) % 2 * Math.PI;

        return ExecutionStatus.SUCCESS;
    }

    public ExecutionStatus setHover(boolean state) {
        this.isHovering = state;

        return ExecutionStatus.SUCCESS;
    }

    public ExecutionStatus setLanding(boolean state) {
        this.isLanding = state;

        return ExecutionStatus.SUCCESS;
    }

    public ExecutionStatus kill() {
        this.isAlive = false;
        consumer.stop();
        bootFlag = BootFlag.BOOT_KILLED;

        return ExecutionStatus.KILLED;
    }

    public ExecutionStatus boot(RemoteBootPackage rbp) {
        try {
            this.iP = new CamImageProvider(imageQueue);
        } catch (CaptureException ex) {
            bootFlag = BootFlag.BOOT_ERROR;
            ce.raiseError(ErrorFlag.BOOT_ERROR);

            return ExecutionStatus.ERROR;
        }

        consumer = new CommandConsumer(commandDrone, ce);
        consumer.start();
        this.isAlive = true;
        bootFlag     = BootFlag.BOOT_SUCCESS;

        return ExecutionStatus.BOOTSTRAP;
    }

    public ExecutionStatus raiseError(ErrorFlag ef) {
        errorFlag = ef;

        return ExecutionStatus.ERROR;
    }

    private ExecutionStatus setBootState(BootFlag bf) {
        bootFlag = bf;

        return ExecutionStatus.ERROR;
    }

    public ExecutionStatus raiseCollisionFlag() {
        collisionFlag = true;

        return ExecutionStatus.EXCEPTION;
    }

    public void startFeed() throws Exception {
        iP.start();
        System.out.println("Started...");
    }

    public void stopFeed() throws Exception {
        iP.stop();
    }

    public RemoteImage takeImage() throws InterruptedException {
        return imageQueue.take();
    }

    public Vector3d getCoords() {
        return new Vector3d(x, y, z);
    }

    public Vector3d getAngles() {
        return new Vector3d(pitch, roll, yaw);
    }

    public boolean isHovering() {
        return isHovering;
    }

    public boolean isLanding() {
        return isLanding;
    }

    public boolean isAlive() {
        return isAlive;
    }

    public ErrorFlag errorState() {
        return errorFlag;
    }

    public boolean hasCollided() {
        return collisionFlag;
    }

    public Demo getEnvironment() {
        return null;
    }

    protected void setAlive(boolean isAlive) {
        this.isAlive = isAlive;
    }

    public BootFlag bootState() {
        return bootFlag;
    }
}


//~ Formatted by Jindent --- http://www.jindent.com
